#include "protocol.h"
#include <string.h>
#include "esp_log.h"
#include <stdio.h>
#include <stdlib.h>
#define TAG "Protocol"

#define PROTO_HEAD           0xAAAAAAAA  /* 协议帧头 */
#define PROTO_HEAD_LEN       4           /* 帧头长度 */
#define PROTO_LENGTH_LEN     2           /* 长度域长度 */
#define PROTO_CMD_LEN        1           /* 命令字长度 */
#define PROTO_CRC_LEN        2           /* CRC校验长度 */

void proto_parser_init(proto_parser_t *parser, uint8_t *buf, uint32_t size)
{
    parser->state = PROTO_STATE_IDLE;
    parser->buf = buf;
    parser->buf_size = size;
    parser->data_len = 0;
    parser->recv_len = 0;
    parser->cmd = 0;
    parser->crc = 0;
}

bool proto_parse_byte(proto_parser_t *parser, uint8_t byte)
{
    bool frame_complete = false;
    static uint32_t head = 0;
    static uint16_t crc = 0;

    switch (parser->state) {
    case PROTO_STATE_IDLE:
        if(byte == (PROTO_HEAD >> 24)) {
            parser->state = PROTO_STATE_HEAD1;
        }
        break;
        
    case PROTO_STATE_HEAD1:
        if(byte == ((PROTO_HEAD >> 16) & 0xFF)) {
            parser->state = PROTO_STATE_HEAD2;
        } else {
            parser->state = PROTO_STATE_IDLE;
        }
        break;
        
    case PROTO_STATE_HEAD2:
        if(byte == ((PROTO_HEAD >> 8) & 0xFF)) {
            parser->state = PROTO_STATE_HEAD3;
        } else {
            parser->state = PROTO_STATE_IDLE;
        }
        break;
        
    case PROTO_STATE_HEAD3:
        if(byte == (PROTO_HEAD & 0xFF)) {
            parser->state = PROTO_STATE_LEN1;
        } else {
            parser->state = PROTO_STATE_IDLE;
        }
        break;
        
    case PROTO_STATE_LEN1:
        parser->data_len = byte;
        parser->state = PROTO_STATE_LEN2;
        break;
        
    case PROTO_STATE_LEN2:
        parser->data_len |= (byte << 8);
        parser->state = PROTO_STATE_LEN3;
        break;
        
    case PROTO_STATE_LEN3:
        parser->data_len |= (byte << 16);
        parser->state = PROTO_STATE_LEN4;
        break;
        
    case PROTO_STATE_LEN4:
        parser->data_len |= (byte << 24);
        parser->state = PROTO_STATE_CMD;
        break;
        
    case PROTO_STATE_CMD:
        parser->cmd = byte;
        if(parser->data_len > 0) {
            parser->state = PROTO_STATE_DATA;
            parser->recv_len = 0;
        } else {
            parser->state = PROTO_STATE_CRC1;
        }
        break;
        
    case PROTO_STATE_DATA:
        parser->buf[parser->recv_len++] = byte;
        if (parser->recv_len >= parser->data_len) {
            parser->state = PROTO_STATE_CRC1;
        }
        break;

    case PROTO_STATE_CRC1:
        crc = byte;
        parser->state = PROTO_STATE_CRC2;
        break;
        
    case PROTO_STATE_CRC2:
        crc |= (byte << 8);
        parser->state = PROTO_STATE_IDLE;
        
        /* 计算并验证CRC */
        // 创建临时缓冲区存放所有需要计算CRC的数据
        uint8_t *crc_buf = malloc(9 + parser->data_len);
        if (!crc_buf) {
            ESP_LOGE(TAG, "Failed to allocate CRC buffer");
            break;
        }
        
        // 填充数据
        int offset = 0;
        // 帧头
        crc_buf[offset++] = (PROTO_HEAD >> 24) & 0xFF;
        crc_buf[offset++] = (PROTO_HEAD >> 16) & 0xFF;
        crc_buf[offset++] = (PROTO_HEAD >> 8) & 0xFF;
        crc_buf[offset++] = PROTO_HEAD & 0xFF;
        // 长度
        crc_buf[offset++] = parser->data_len & 0xFF;
        crc_buf[offset++] = (parser->data_len >> 8) & 0xFF;
        crc_buf[offset++] = (parser->data_len >> 16) & 0xFF;
        crc_buf[offset++] = (parser->data_len >> 24) & 0xFF;
        // 命令字
        crc_buf[offset++] = parser->cmd;
        // 数据
        memcpy(crc_buf + offset, parser->buf, parser->data_len);
        offset += parser->data_len;
        
        // 计算CRC
        uint16_t calc_crc = crc16(crc_buf, offset);
        free(crc_buf);
        
        if (calc_crc == crc) {
            frame_complete = true;
        } else {
            ESP_LOGE(TAG, "CRC check failed");
        }
        break;

    default:
        break;
    }

    return frame_complete;
}

int proto_pack(uint8_t *buf, int buf_size, uint8_t cmd, const uint8_t *data, uint32_t len)
{
    if(buf_size < len + 11) {  // 头(4) + 长度(4) + 命令(1) + CRC(2)
        return -1;
    }
    
    int offset = 0;
    
    // 写入帧头
    buf[offset++] = (PROTO_HEAD >> 24) & 0xFF;
    buf[offset++] = (PROTO_HEAD >> 16) & 0xFF;
    buf[offset++] = (PROTO_HEAD >> 8) & 0xFF;
    buf[offset++] = PROTO_HEAD & 0xFF;
    
    // 写入长度
    buf[offset++] = len & 0xFF;
    buf[offset++] = (len >> 8) & 0xFF;
    buf[offset++] = (len >> 16) & 0xFF;
    buf[offset++] = (len >> 24) & 0xFF;
    
    // 写入命令字
    buf[offset++] = cmd;
    
    // 写入数据
    if(len > 0 && data != NULL) {
        memcpy(buf + offset, data, len);
        offset += len;
    }
    
    // 计算并写入CRC
    uint16_t crc = crc16(buf, offset);
    buf[offset++] = crc & 0xFF;
    buf[offset++] = (crc >> 8) & 0xFF;
    
    return offset;
}

/* CRC16计算函数 */
uint16_t crc16(const uint8_t *data, uint32_t len)
{
    uint16_t crc = 0xFFFF;
    uint32_t i;
    uint8_t j;
    
    for (i = 0; i < len; i++) {
        crc ^= data[i];
        for (j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc = (crc >> 1) ^ 0xA001;
            } else {
                crc = crc >> 1;
            }
        }
    }
    
    return crc;
}

static uint16_t calc_crc16(const uint8_t *data, uint16_t len)
{
    uint16_t crc = 0xFFFF;
    for (uint16_t i = 0; i < len; i++) {
        crc ^= data[i];
        for (uint8_t j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc = (crc >> 1) ^ 0xA001;
            } else {
                crc = crc >> 1;
            }
        }
    }
    return crc;
} 